#include "ArmourPlate.h"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <iostream>
#include <vector>

using namespace std;
using namespace cv;

/****************************************图像预处理模块开始*******************************************/
/**************************************************************************************************/


/************************************************************************************************
* 函数名：预处理函数：Pretreatment
* 函数功能描述：子类中Pretreatment重写函数，对图像进行初步的处理，比如降噪、二值化
* 函数参数：Mat矩阵类，为未经处理的BGR图像
* 函数返回值：Mat矩阵类，为处理后的二值图
* 作者：朱荣辉
* 修改日期：暂无
* 修改原因：暂无
************************************************************************************************/
Mat ArmourPlate :: Pretreatment(Mat frame) {
	if (frame.empty()) {
		cout << "Something is wrong!";
	}
	const int LowH = 11;         
	const int HighH = 34;
	const int LowS = 43;
	const int HighS = 255;
	const int LowV = 46;
	const int HighV = 255;
	Mat hsv, binary;		//HSV和二值图
	cvtColor(frame, hsv,COLOR_BGR2HSV);		//bgr转hsv图
	//inRange(hsv, Scalar(78, 43, 46), Scalar(124, 255, 255), binary);
	inRange(hsv, Scalar(LowH, LowS, LowV), Scalar(HighH, HighS, HighV), binary);	//依据我们设定的参数进行二值化
	Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
	erode(binary, binary, element, Point(-1, -1), 1);
	dilate(binary, binary, element, Point(-1, -1), 1);
	return binary;
}

Mat ArmourPlate :: whiteLightblur_blue(const Mat frame)
{
    vector<Mat> temp;
    split(frame, temp);//颜色通道分离
    Mat frame_have;
    frame_have = 2 * temp[0];//进行通道加减
    frame_have -= (temp[2] * 0.5 + temp[1] * 0.5);
    threshold(frame_have, frame_have, 140, 255, THRESH_BINARY);//阈值处理
    return frame_have;
}

Mat ArmourPlate :: whiteLightblur_red(const Mat frame)
{
    vector<Mat> temp;
    split(frame, temp);//颜色通道分离
    Mat frame_have;
    frame_have = (temp[2] * 0.5 + temp[1] * 0.5) * 1.3;//进行通道加减
    frame_have -= temp[0];
    threshold(frame_have, frame_have, 160, 255, THRESH_BINARY);//阈值处理
    return frame_have;
}

/****************************************图像预处理模块结束*******************************************/
/************************************************************************************************/



/**********************************卡尔曼滤波模块开始********************************************/
const int stateNum = 6;					//当前状态值6×1向量(x,y)
const int measureNum = 6;                               //测量值6×1向量(x,y)	
KalmanFilter KF(stateNum, measureNum, 0);		//实例化卡尔曼滤波类
int T = 1;						//卡尔曼滤波参考系下的时间
Point2f predict_point;					//基于卡尔曼滤波预测的预测点
int sumT = 0;						//卡尔曼滤波参考系下的时间的总和（用于算法）
int T_time[3];						//当前帧、上一帧、上上一帧的时间
int t_count = 0;					//计数
Point2f llastp, lastp, nowp;				//储存当前帧、上一帧、上上一帧的点信息
Mat measurement = Mat::zeros(measureNum, 1, CV_32F);

/*********************************************************************************************
	*函数名：卡尔曼滤波初始化  kalman_init
	*函数功能描述：（非虚函数，子类可以直接继承使用）对卡尔曼滤波需要的几个参数进行初始化
	* 函数参数：无
	* 函数返回值：无
	* 作者：朱荣辉
	* 修改日期：2022.5.14
	* 修改原因：新增加速度模型
	******************************************************************************************/

void ArmourPlate :: kalman_init() {
	KF.transitionMatrix = (Mat_<float>(stateNum, measureNum) <<
		1, 0, T, 0, 1 / 2 * T * T, 0,
		0, 1, 0, T, 0, 1 / 2 * T * T,
		0, 0, 1, 0, T, 0,
		0, 0, 0, 1, 0, T,
		0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 1);  //转移矩阵A

	setIdentity(KF.measurementMatrix);                                  //设置测量矩阵H
	setIdentity(KF.processNoiseCov, Scalar::all(5e-4));                 //设置系统噪声方差矩阵Q
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-3));             //设置测量噪声方差矩阵R
	setIdentity(KF.errorCovPost, Scalar::all(1));                       //设置后验错误估计协方差矩阵P

}

/****************************************************************************************
	*函数名：卡尔曼预测函数  kalman_predict
	*函数功能描述：对当前位置点进行下一帧的预测，返回预测的坐标点
	* 函数参数：Point2f 类型点，为当前装甲板所在的位置的中心点
	* 函数返回值：基于传入的中心点的预测点
	* 作者：朱荣辉
	* 修改日期：2022.5.14
	* 修改原因：使用时间轴存储相对时间，得以引入加速度修正预测
	************************************************************************************/

Point2f ArmourPlate :: kalman_predict(Point2f target_centre) {
	Mat prediction = KF.predict();			                //预测	
	llastp = lastp;					                //在新的一帧里，更新点的信息
	lastp = nowp;
	nowp = target_centre;
	predict_point = Point(prediction.at<float>(0), prediction.at<float>(1));//获取预测值(x',y')
	measurement.at<float>(0) = (float)target_centre.x;			//更新测量值
	measurement.at<float>(1) = (float)target_centre.y;
	measurement.at<float>(2) = (float)nowp.x - lastp.x;                     //速度
	measurement.at<float>(3) = (float)nowp.y - lastp.y;
	measurement.at<float>(4) = (nowp.x - lastp.x) - (lastp.x - llastp.x);   //加速度
	measurement.at<float>(5) = (nowp.y - lastp.y) - (lastp.y - llastp.y);
	KF.correct(measurement);			                        //根据测量值修正协方差矩阵
	return predict_point;				                        //返回预测的点
}

/*************************************卡尔曼滤波模块结束*******************************************/



vector<Rect> ArmourPlate:: matchingForArmo(vector<vector<Point>>& contours) {
	if (contours.empty()) {
		cout << "Conturn is empty!No target!";
	}
	vector<Rect>m_lights;
	
	int size = contours.size();
	//cout << "match_size  " << size << endl;
	if (contours.size() == 2) {                                	//若只有两个轮廓，视为已经对准装甲板，直接返回结果。			
		Rect temp1 = boundingRect(contours[0]);
		Rect temp2 = boundingRect(contours[1]);
		Rect temp = temp1 | temp2;
		m_lights.push_back(temp);
		return m_lights;
	}
	for (int i = 0; i < size; i++) {
		for (int j = i + 1; j < size; j++) {
			Rect temp1 = boundingRect(contours[i]);         //左灯条的最小竖直外接矩形
			Rect temp2 = boundingRect(contours[j]);         //右灯条的最小竖直外接矩形
			Rect temp = temp1 | temp2;                      //取并集
			double ratio = temp.height > temp.width ? (double)temp.height / temp.width : (double)temp.width / temp.height;
			//cout << "这是第" << i << "个灯条和第" << j << "个匹配的结果:";
		
			if (temp.width < temp.height) {	                //考虑到没有竖立的装甲板，所以宽应该大于长		
				//cout << "第一种失败！" << endl;
				continue;
			}
			RotatedRect temp3 = minAreaRect(contours[i]);
			RotatedRect temp4 = minAreaRect(contours[j]);
                        /*灯条的面积虽然不确定，但相对面积应该在1附近，所以如果两个轮廓的相对面积相差5倍，就该排除*/
			if (temp3.size.area() / temp4.size.area() < 0.2 || temp3.size.area() / temp4.size.area() > 5) {
				//cout << "相对面积匹配失败" << endl;
				continue;
			}
                        /*我认为灯条的角度差距不能太大，所以如果差超过20度，考虑排除*/
			if (abs(temp3.angle - temp4.angle)>20&&abs(temp3.angle - temp4.angle) <80) {
				//cout << "角度约束未通过" << endl;
				continue;
			}
                        /*摄像头的画幅是640*.60，我认为灯条的y坐标相差>100，即为相差1/3个屏幕，显然不合理，考虑排除*/
			if (abs(temp3.center.y - temp4.center.y) > 100) {
				//cout << "第二种失败！" << endl;
				continue;
			}
                        /*装甲板的长宽高大概是2.57，所以在2.2-3之间挑选目标比较合理*/
			if (ratio < 2.20 || ratio>3) {
				//cout << "第三种失败！" << endl;
				continue;
			}
			else {
				//cout << "成功！"<<endl;
			}
			m_lights.push_back(temp);
		}
	}
        //返回成功的匹配
	return m_lights;
}

/***********************************************************************************************************************
    *函数名：筛选函数：Screen
    *函数功能描述：从所有矩形轮廓中筛除出不可能是装甲板轮廓的矩形
    * 函数参数：处理好的二值图
    * 函数返回值：vector<vector<Point>>，点集，也就是轮廓信息
    * 作者：温浩然
    * 修改日期：暂无
    * 修改原因：暂无
    ************************************************************************************************************************/
vector<vector<Point>> ArmourPlate::Screen(Mat pic)
{
        vector<vector<Point>>contours, contours_temp;
        vector<Vec4i>hierachy;
        vector<int> erase_temp;
        findContours(pic, contours, hierachy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
        //cout << contours.size() << endl;
        bool judge = true;
        contours_temp = contours;
        for (int i = 0; i < contours.size(); i++)//长宽比筛选
        {
                RotatedRect minArea = minAreaRect(contours[i]);
                float height = minArea.size.height;
                float width = minArea.size.width;
                if (height < width)
                {
                        swap(height, width);
                }
                float rate = height / width;
                //cout << "过滤算法r" <<rate<< endl;
                if (rate < 3.3 || rate > 5)
                {
                        //contours.erase(contours.begin() + (erase_temp[i] - i));
                        erase_temp.push_back(i);
                }
        }
        
        int num = erase_temp.size();
        for (int i = 0; i < num; i++)
        {
                contours.erase(contours.begin() + (erase_temp[i] - i));
        }
        if (contours.size() < 2)//轮廓集为空则判定筛除失败
        {
                //cout << "长宽比筛选失败" << endl;
                return contours_temp;
        }
        contours_temp = contours;
        erase_temp.clear();
        for (int i = 0; i < contours.size(); i++)//轮廓和轮廓最小矩形的面积比
        {
                RotatedRect minArea = minAreaRect(contours[i]);
                float area = contourArea(contours[i]);
                float rate = minArea.size.area() / area;
                if (rate < 1 || rate > 1.65)
                {
                        erase_temp.push_back(i);
                }
        }
        num = erase_temp.size();
        for (int i = 0; i < num; i++)
        {
                contours.erase(contours.begin() + (erase_temp[i] - i));
        }
        if (contours.size() < 2)
        {
                //cout << "面积比筛选失败" << endl;
                return contours_temp;
        }
        contours_temp = contours;
        erase_temp.clear();
        for (int i = 0; i < contours.size(); i++)//凸度筛选
        {
                vector<vector<Point>>HullPoint(contours.size());
                float area = contourArea(contours[i]);
                convexHull(Mat(contours[i]), HullPoint[i], false);
                float solidity = contourArea(HullPoint[i]) / area;
                if (solidity < 0.5)
                {
                        erase_temp.push_back(i);
                }
        }
        num = erase_temp.size();
        for (int i = 0; i < num; i++)
        {
                contours.erase(contours.begin() + (erase_temp[i] - i));
        }
        if (contours.size() < 2)
        {
                //cout << "凸度筛选失败" << endl;
                return contours_temp;
        }
        erase_temp.clear();
        contours_temp = contours;
        for (int i = 0; i < contours.size(); i++)//高度差筛选
        {
                int erase = 1;
                RotatedRect minArea1 = minAreaRect(contours[i]);
                for (int j = 0; j < contours.size(); j++)//两两对比高度差
                {
                        if (i == j)
                        {
                                continue;
                        }
                        RotatedRect minArea2 = minAreaRect(contours[j]);
                        float minus = abs(minArea1.center.y - minArea2.center.y);//计算两个轮廓之间的高度差
                        if (minus < 32)
                        {
                                erase = 0;
                        }
                }
                if (erase == 1)
                {
                        erase_temp.push_back(i);
                }
        }
        num = erase_temp.size();
        for (int i = 0; i < num; i++)
        {
                contours.erase(contours.begin() + (erase_temp[i] - i));
        }
        if (contours.size() < 2)
        {
                //cout << "高度差筛选失败" << endl;
                return contours_temp;
        }
        return contours;
}